#include <avr/io.h>
#include <avr/interrupt.h>
#include <inttypes.h>


void init_motors(void){

    //Poorten als input/output defini�ren
	DDRD |= _BV(PD3); //PIN3 van PORTD output
	DDRD |= _BV(PD5); //PIN5 van PORTD output
	DDRD |= _BV(PD6); //PIN6 van PORTD output

    /*Settings voor Pulse Width Modulation
	  Configuration Timer Counter Control Registers */
	TCCR0A = 0b10100001;
	TCCR0B = 0b00000001;
	TCCR2A = 0b00100001;
	TCCR2B = 0b00000001;

	//Configuration Timer Counter Interrupt Mask register:
	TIMSK0= (0 << OCIE0B); //Timer0 Output Compare Match 0B
	TIMSK0= (0 << OCIE0A); //Timer0 Output Compare Match 0A
	TIMSK0= (0 << TOIE0);  //Timer0 Overflow Interrupt

	TIMSK2= (0 << OCIE2B); //Timer2 Output Compare Match 2B
	TIMSK2= (0 << OCIE2A); //Timer2 Output Compare Match 2A
	TIMSK2= (0 << TOIE0);  //Timer2 Overflow Interrupt


    //Settings Value Timer Counter Registers
    TCNT0=0; //Always starts Register 0 counting from 0
    TCNT2=0; //Always starts Register 2 counting from 0

    //Settings Value Output Compare Register
    OCR0A=0;  //The OCR0A is continuously compared with TCNT0
    OCR0B=0; //The OCR0B is continuously compared with TCNT0
    OCR2B=0; //The OCR2B is continuously compared with TCNT2

}

unsigned char get_speed_motor(unsigned char motor){
	switch(motor){
		case 1:
			return OCR0A;
			break;
		case 2:
			return OCR0B;
			break;
		case 3:
			return OCR2B;
			break;
	}
	return 0;
}

void set_main_rotors(unsigned char throttle, signed char delta){
	if(throttle <= 155){
		OCR2B	= 0;
		OCR0B	= 0;
		return;
	}

	if(delta < 0){
		//bovenrotor == OCR2B
		//onderrotor == OCR0B
		OCR2B	= throttle;
		OCR0B	= throttle + delta;
	}else{
		OCR0B	= throttle;
		OCR2B	= throttle - delta;

	}
}

void set_tail_rotor(unsigned char speed, unsigned char main_throttle){
	if(main_throttle <= 155){
		OCR0A	= 0;
	}else{
		if ( speed > 100){
			speed	= 100;
		}
		if (speed > 0){
			speed += 155;
		}
		OCR0A	= speed;
	}
}

unsigned char set_speed_motor(unsigned char motor,unsigned char speed){
	if ( speed > 100){
		speed=100;
	}

	if (speed>0)
		speed+=125;

	switch(motor){
		case 1:
			OCR0A = speed;
			break;
		case 2:
			OCR0B = speed;
			break;
		case 3:
			OCR2B = speed;
			break;
	}
	return 0;
}
